#ifndef CARTOGRAPHER_ADAPTIVE_MATCHER_ANGLE_2D_H_
#define CARTOGRAPHER_ADAPTIVE_MATCHER_ANGLE_2D_H_

#include "cartographer/mapping/internal/2d/scan_matching/adaptive_matcher.h"

namespace cartographer
{
    class AdaptiveFilter
    {
    public:
        using Ptr = std::shared_ptr<AdaptiveFilter>;

        AdaptiveFilter(const ScanMatchOptions &options);

        void Failed();

        bool Check();

        int Resample();

        void UpdateRotate();

        void SampleNears();

        inline int GetLinearStepSize()
        {
            resolution_linear = 0.01;
            //  XY方向的搜索步长
            return std::ceil(win_linear / resolution_linear);
        }

        inline Eigen::Vector3d GetOffsetDelta()
        {
            return offsetExpect - offsetBest;
        }

        const Candidate2D &GetBestCand();

        void UpdateWeightByExp();

        void ResetLinearBounds(const int num_scans, const int step_size);

        void UpdateParameter(const float max_range, const double submap_resolution);

        void ScoreCandidates(const mapping::Grid2D &grid, const std::vector<DiscreteScan2D> &discrete_scans);

        void UpdateRotateByCandidates();

        int Success(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt);

        bool inited_ = false;
        int num_scans_;
        int best_idx_ = 0;

        double win_linear;
        double win_angular;
        double win_def_linear;
        double win_def_angular;
        double cost_weight_linear = 0;
        double cost_weight_angular = 0;

        double resolution_linear;
        double angular_stride;

        double alpha_slow_ = 0.3;
        double alpha_fast_ = 0.7;
        double weight_slow_ = -1;
        double weight_fast_ = -1;

        Eigen::Vector3d offsetBest;
        Eigen::Vector3d offsetExpect;
        Eigen::Vector3i offsetStep;

        std::vector<double> weights_;
        std::vector<double> rotations_;
        std::vector<Candidate2D> candidates;
        std::vector<LinearBounds> linear_bounds;

        GaussComputer gauss_;
    };

    class AdaptiveMatcherAngle2D : public RealTimeScanMatcher2D
    {
    public:
        explicit AdaptiveMatcherAngle2D(const ScanMatchOptions &options);
        AdaptiveMatcherAngle2D(const AdaptiveMatcherAngle2D &) = delete;
        AdaptiveMatcherAngle2D &operator=(const AdaptiveMatcherAngle2D &) = delete;

        double Match(const transform::Rigid2d &initial_pose_estimate, const sensor::PointCloud &point_cloud, const mapping::Grid2D &grid,
                     transform::Rigid2d *pose_estimate);

        void Failed();

        int Success(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt);

    private:
        const ScanMatchOptions options_;

        AdaptiveFilter::Ptr filter_;
    };
} // namespace cartographer

#endif